#!/usr/bin/env python
import roslib; roslib.load_manifest('beginner_tutorials')
import rospy
import serial
from time import sleep
from std_msgs.msg import Int8
from sensor_msgs.msg import RegionOfInterest

x_pos = 128	#Variables globales para mantener la posición
y_pos = 128	#de la camara

def callback(data):

	pos_x_center = 320 #Constantes con la posición del centro
	pos_y_center = 240 #de la camara
	
	x_center = data.x_offset + (data.width/2) #Calcula el punto medio de la posición de la cara
	y_center = data.y_offset + (data.height/2)
	#rospy.loginfo(": I heard %s" % x_center + " and %s" % y_center) #Log de la info calculada para debug
	
	if (pos_x_center - x_center) > 15: x_pos -= 1	#Control proporcional
	if (pos_x_center - x_center) < -15: x_pos += 1	#Aumenta o disminuye la posicion de los motores
	if (pos_y_center - y_center) > 15: y_pos += 1	#dependiendo del error
	if (pos_y_center - y_center) < -15: y_pos -= 1	
	
	if (x_pos > 255): x_pos = 255	#Corrige excesos en el movimiento
	if (x_pos < 0): x_pos = 0
	if (y_pos > 255): y_pos = 255
	if (y_pos < 0): y_pos = 0	

	s.write("\xFF\x0B")	#Envia por serial
	s.write(chr(x_pos))
	s.write("\xFF\x0A")
	s.write(chr(y_pos))
	

def listener():	#Listener tipico de ROS
	rospy.init_node('listener', anonymous=True)
	rospy.Subscriber("roi", RegionOfInterest, callback)
	rospy.spin()

if __name__ == '__main__':
	s = serial.Serial() #Configuración Serial
	s.port = "/dev/ttyS0"
	s.baudrate = 9600
	s.open()
	if s.isOpen(): 	
		print "serial open"	
	listener()
